#include "stair_ctrl/stair_ctrl.hpp"

int main(int argc, char **argv)
{
    ros::init(argc, argv, "stair_ctrl");
    ros::NodeHandle nh("~");

    std::string low_state_topic, low_cmd_topic, joy_stick_topic, reset_topic, state_machine_topic, nn_state_topic, nn_param_path, nn_log_path;
    nh.param<std::string>("low_state_topic", low_state_topic, "/ros_udp_comm_low/low_state");
    nh.param<std::string>("low_cmd_topic", low_cmd_topic, "low_cmd");
    nh.param<std::string>("joy_stick_topic", joy_stick_topic, "joy_stick");
    nh.param<std::string>("reset_topic", reset_topic, "reset");
    nh.param<std::string>("state_machine_topic", state_machine_topic, "state");
    nh.param<std::string>("nn_state_topic", nn_state_topic, "nn_state");
    nh.param<std::string>("nn_param_path", nn_param_path, "");
    nh.param<std::string>("nn_log_path", nn_log_path, "");

    stairCtrl stair_ctrl = stairCtrl(&nh, low_state_topic, low_cmd_topic, joy_stick_topic, reset_topic, state_machine_topic, nn_state_topic, nn_param_path, nn_log_path);
    stair_ctrl.start();
}